/**
 * @file        bot_car.c
 * @brief       XXXX
 * @note        XXXX
 * @author      靳普诏(puzhao.jin@hopechart.com)
 * @date        2024/04/02
 * @version     1.0

 * @par         修改日志
 * <table>
 * <tr><th>Date         <th>Version     <th>Author      <th> Description
 * <tr><td>2024/04/02   <td>1.0         <td>靳普诏       <td> 创建初始版本
 * @copyright 杭州鸿泉物联网技术股份有限公司
 */
 
#include "bot_car.h"

#include <string.h>
#include <math.h>

// ---------------------------------------------------------------
static Bool L_Bot_CompPlace(const Bot_TCarPlace *a, const Bot_TCarPlace *b)
{
    return a->d == b->d 
           && a->x == b->x
           && a->y == b->y;
}

static Int32 L_Bot_AngleValid(Int32 d)
{
    Int32 result = 0;
    const Int32 angle_max = 360000;
    
    if (d >= 0)
    {
        result = (d % angle_max);
    }
    else
    {
        d = -d;
        result = angle_max - (d % angle_max);
    }


    return result;
}

/**
 * 角度减法 a - b 
 * @param a
 * @param b
 * @return 
 */
static Int32 L_Bot_AngleDiff(Int32 a, Int32 b)
{
    // 归 0 
    b -= a;
    a = 0;

    b = L_Bot_AngleValid(b);

    if (b >= 180000)
    {
        a = b - 360000;
    }
    else
    {
        a = b;
    }

    return a;
}

Int32 L_Bot_CalcDistance(Int32 x1, Int32 y1, Int32 x2, Int32 y2)
{
    Int32 dx = x2 - x1;
    Int32 dy = y2 - y1;
    Int32 distance = (Int32)sqrt(dx * dx + dy * dy);

    return distance;
}

Int32 L_Bot_CalcPlaceAngle(Int32 x1, Int32 y1, Int32 x2, Int32 y2)
{
    // 计算 y2 - y1 和 x2 - x1
    Int32 dy = y2 - y1;
    Int32 dx = x2 - x1;

    // 使用 atan2 函数计算角度（弧度）
    double angleRadians = atan2((double) dy, (double) dx);

    // 将弧度转换为度
    double angleDegrees = angleRadians * 180.0 / 3.141592654;

    // 将度转换为0.001度的单位
    Int32 angleMilliDegrees = (Int32)(angleDegrees * 1000.0);

    // 确保角度在 0 到 360000 度之间
    return L_Bot_AngleValid(angleMilliDegrees);
}

// ---------------------------------------------------------------

static void L_Bot_TCarDoStateNotify(Bot_TCar *self, PChar error_info)
{
    Bot_TCarDoStateNotifyMethod func = self->DoStateNotify;

    self->error_info_ = error_info;

    if (func != NULL)
    {
        func(self, self->state);
    }
}

static Bool L_Bot_TCarDoGetCurrPlace(Bot_TCar *self, Bot_TCarPlace *ret_place)
{
    Bot_TCarDoGetCurrPlaceMethod func = self->DoGetCurrPlace;
    Bool result = False;

    if (func != NULL)
    {
        result = func(self, ret_place);
    }

    return result;
}

static Bool L_Bot_TCarDoSetCurrPlace(Bot_TCar *self, const Bot_TCarPlace *place)
{
    Bot_TCarDoSetCurrPlaceMethod func = self->DoSetCurrPlace;
    Bool result = False;

    if (func != NULL)
    {
        result = func(self, place);
    }

    return result;
}

static Bool L_Bot_TCarDoSetMotivePwr(Bot_TCar *self, Int32 angle, Int32 distance)
{
    Bot_TCarDoSetMotivePwrMethod func = self->DoSetMotivePwr;
    Bool result = False;

    if (func != NULL)
    {
        result = func(self, angle, distance);
    }

    return result;
}

static Bool L_Bot_TCarDoSetSpinPwr(Bot_TCar *self, Int32 diff_angle)
{
    Bot_TCarDoSetSpinPwrMethod func = self->DoSetSpinPwr;
    Bool result = False;

    if (func != NULL)
    {
        result = func(self, diff_angle);
    }

    return result;
}

static void L_Bot_TCarDoRunOnce(Bot_TCar *self)
{
    Bot_TCarDoRunOnceMethod func = self->DoRunOnce;

    if (func != NULL)
    {
        func(self);
    }
}

static Bool L_Bot_TCarDoCheckPlaceValid(Bot_TCar *self, const Bot_TCarPlace *in_place)
{
    Bot_TCarDoCheckPlaceValidMethod func = self->DoCheckPlaceValid;
    Bool result = False;

    if (func != NULL)
    {
        result = func(self, in_place);
    }

    return result;
}

// ---------------------------------------------------------------

static void L_Bot_TCarCalcMotivePwr(Bot_TCar *self)
{
    if (L_Bot_CompPlace(&self->curr_place_, &self->next_place_))
    {
        self->motive_distance_ = 0;
        self->motive_angle_ = 0;
    }
    else
    {
        Int32 x1, y1, x2, y2, abs_angle;
        
        x1 = self->curr_place_.x;
        y1 = self->curr_place_.y;
        x2 = self->next_place_.x;
        y2 = self->next_place_.y;

        self->motive_distance_ = L_Bot_CalcDistance(x1, y1, x2, y2);
        abs_angle = L_Bot_CalcPlaceAngle(x1, y1, x2, y2);

        // 换算成相对小车角度
        self->motive_angle_ = L_Bot_AngleDiff(abs_angle, self->curr_place_.d);
        
    }
}

static void L_Bot_TCarCalcSpinPwr(Bot_TCar *self)
{
    Bot_TCarPlace curr_place = self->curr_place_;
    Bot_TCarPlace next_place = self->next_place_;
    
    if (curr_place.d != next_place.d)
    {
        self->spin_diff_angle_ = L_Bot_AngleDiff(curr_place.d, next_place.d);
    }
    else
    {
        self->spin_diff_angle_ = 0;
    }
}


static void Bot_TCarRunOnceByPwrMode(Bot_TCar *self)
{
    L_Bot_TCarCalcSpinPwr(self);
    L_Bot_TCarDoSetSpinPwr(self, self->spin_diff_angle_);
    
    L_Bot_TCarDoSetMotivePwr(self, self->motive_angle_, self->motive_distance_);
}

static void Bot_TCarRunOnceByPlaceMode(Bot_TCar *self)
{
    L_Bot_TCarCalcSpinPwr(self);
    L_Bot_TCarDoSetSpinPwr(self, self->spin_diff_angle_);

    L_Bot_TCarCalcMotivePwr(self);
    L_Bot_TCarDoSetMotivePwr(self, self->motive_angle_, self->motive_distance_);
}

// ---------------------------------------------------------------

Int32 Bot_TCarCreate(Bot_TCar *self)
{
    Int32 result = -1;

    if (self != NULL)
    {
        memset(self, 0, sizeof(*self));

        self->state = (Bot_kCarStateMaskValid | Bot_kCarStateMaskArrived);
        result = self->state;
        L_Bot_TCarDoStateNotify(self, "Bot_TCarCreate");
    }   

    return result;
}

void Bot_TCarRunOnce(Bot_TCar *self)
{
    self->last_place_ = self->curr_place_;
    if (!L_Bot_TCarDoGetCurrPlace(self, &self->curr_place_))
    {
        self->curr_place_ = self->last_place_;
    }

    if ((self->state & Bot_kCarStateMaskPlaceMod) == 0)
    {
        Bot_TCarRunOnceByPwrMode(self);

        if (L_Bot_CompPlace(&self->curr_place_, &self->last_place_))
        {
            if ((self->state & Bot_kCarStateMaskArrived) == 0)
            {
                self->state |= Bot_kCarStateMaskArrived;
                L_Bot_TCarDoStateNotify(self, NULL);
            }
        }
        else
        {
            if ((self->state & Bot_kCarStateMaskArrived) != 0)
            {
                self->state &= ~Bot_kCarStateMaskArrived;
                L_Bot_TCarDoStateNotify(self, NULL);
            }
        }
    }
    else
    {
        Bot_TCarRunOnceByPlaceMode(self);

        if (L_Bot_CompPlace(&self->curr_place_, &self->next_place_))
        {
            self->state |= Bot_kCarStateMaskArrived;
            L_Bot_TCarDoStateNotify(self, NULL);
        }   
        else
        {
            if ((self->state & Bot_kCarStateMaskArrived) != 0)
            {
                self->state &= ~Bot_kCarStateMaskArrived;
                L_Bot_TCarDoStateNotify(self, NULL);
            }
        }    
    }
    
    L_Bot_TCarDoRunOnce(self);
}

void Bot_TCarDestroy(Bot_TCar *self)
{
    if (self != NULL)
    {
        Bot_TCarSetMotivePwr(self, 0, 0);
        self->state = 0;
        L_Bot_TCarDoStateNotify(self, "Bot_TCarDestroy");
    }
}


UInt32 Bot_TCarState(Bot_TCar *self)
{
    return self->state;
}


Bool Bot_TCarSetPlaceMod(Bot_TCar *self, Bool is_place_mode)
{
    
    if (is_place_mode)
    {
        L_Bot_TCarDoGetCurrPlace(self, &self->curr_place_);
        self->state |= Bot_kCarStateMaskPlaceMod;
    }
    else
    {
        self->state &= ~Bot_kCarStateMaskPlaceMod;
    }
    
    return True;
}

Bool Bot_TCarSetMotivePwr(Bot_TCar *self, Int32 angle, Int32 distance)
{
    Bool result = False;

    if ((self->state & Bot_kCarStateMaskPlaceMod) == 0)
    {
        self->motive_angle_ = angle;
        self->motive_distance_ = distance;
        result = L_Bot_TCarDoSetMotivePwr(self, angle, distance);
    }

    return result;
}

Bool Bot_TCarSetSpinPwr(Bot_TCar *self, Int32 diff_angle)
{
    Bool result = False;

    if ((self->state & Bot_kCarStateMaskPlaceMod) == 0)
    {
        self->spin_diff_angle_ = diff_angle;
        result = L_Bot_TCarDoSetSpinPwr(self, diff_angle);
    }

    return result;
}

Bool Bot_TCarSetPlace(Bot_TCar *self, const Bot_TCarPlace *place, Bool is_next_place)
{
    Bool result = False;

    if (is_next_place)
    {
        if ((self->state & Bot_kCarStateMaskPlaceMod) != 0)
        {
            if (!L_Bot_CompPlace(place, &self->next_place_))
            {
                self->next_place_ = *place;
            }
        }
    }
    else
    {
        result = L_Bot_TCarDoSetCurrPlace(self, place);
        if (result)
        {
            self->last_place_ = *place;
            self->curr_place_ = *place;
        }
    }

    return result;
}
Bool Bot_TCarGetPlace(Bot_TCar *self, Bot_TCarPlace *r_place, Bool is_next_place)
{
    Bool result = False;

    if (r_place == NULL)
        ;
    else if (is_next_place)
    {
        if ((self->state & Bot_kCarStateMaskPlaceMod) != 0)
        {
            *r_place = self->next_place_;
            result = True;
        }
    }
    else
    {
        if ((self->state & Bot_kCarStateMaskPlaceMod) != 0)
        {
            *r_place = self->curr_place_;
            result = True;
        }
        else
        {
            result = L_Bot_TCarDoGetCurrPlace(self, r_place);
        }
    }

    return result;
}


PChar Bot_TCarGetErrorInfo(Bot_TCar *self)
{
    PChar result = NULL;

    if (self != NULL)
    {
        result = self->error_info_;
    }

    return result;
}







///< Generated on "2024-04-02 21:49:29" by the tool "gen_hq_file.py >> V20231119" 

